/*
 * Operator.c
 *
 *  Created on: Jan 27, 2025
 *      Author: hzy
 */

#include "Operator.h"
#include "debug.h"

void OperatorForward()
{
    // MotorA
    GPIO_WriteBit(GPIOC, GPIO_Pin_6, 1);
    GPIO_WriteBit(GPIOC, GPIO_Pin_7, 0);

    // MotorB
    GPIO_WriteBit(GPIOC, GPIO_Pin_0, 1);
    GPIO_WriteBit(GPIOD, GPIO_Pin_3, 0);

    //MotorC
    GPIO_WriteBit(GPIOC, GPIO_Pin_2, 0);
    GPIO_WriteBit(GPIOC, GPIO_Pin_5,1);

    //MotorD
    GPIO_WriteBit(GPIOD, GPIO_Pin_2, 1);
    GPIO_WriteBit(GPIOC, GPIO_Pin_1, 0);
}

void OperatorBackward()
{
    // MotorA
    GPIO_WriteBit(GPIOC, GPIO_Pin_6, 0);
    GPIO_WriteBit(GPIOC, GPIO_Pin_7, 1);

    // MotorB
    GPIO_WriteBit(GPIOC, GPIO_Pin_0, 0);
    GPIO_WriteBit(GPIOD, GPIO_Pin_3, 1);

    //MotorC
    GPIO_WriteBit(GPIOC, GPIO_Pin_2, 1);
    GPIO_WriteBit(GPIOC, GPIO_Pin_5,0);

    //MotorD
    GPIO_WriteBit(GPIOD, GPIO_Pin_2, 0);
    GPIO_WriteBit(GPIOC, GPIO_Pin_1, 1);
}

void OperatorTurnLeft()
{
    // MotorA
    GPIO_WriteBit(GPIOC, GPIO_Pin_6, 1);
    GPIO_WriteBit(GPIOC, GPIO_Pin_7, 0);

    // MotorB
    GPIO_WriteBit(GPIOC, GPIO_Pin_0, 1);
    GPIO_WriteBit(GPIOD, GPIO_Pin_3, 0);

    //MotorC
    GPIO_WriteBit(GPIOC, GPIO_Pin_2, 1);
    GPIO_WriteBit(GPIOC, GPIO_Pin_5, 0);

    //MotorD
    GPIO_WriteBit(GPIOD, GPIO_Pin_2, 0);
    GPIO_WriteBit(GPIOC, GPIO_Pin_1, 1);
}

void OperatorTurnRight()
{
    // MotorA
    GPIO_WriteBit(GPIOC, GPIO_Pin_6, 0);
    GPIO_WriteBit(GPIOC, GPIO_Pin_7, 1);

    // MotorB
    GPIO_WriteBit(GPIOC, GPIO_Pin_0, 0);
    GPIO_WriteBit(GPIOD, GPIO_Pin_3, 1);

    //MotorC
    GPIO_WriteBit(GPIOC, GPIO_Pin_2, 0);
    GPIO_WriteBit(GPIOC, GPIO_Pin_5, 1);

    //MotorD
    GPIO_WriteBit(GPIOD, GPIO_Pin_2, 1);
    GPIO_WriteBit(GPIOC, GPIO_Pin_1, 0);
}

void OperatorTurnAround()
{
    OperatorTurnLeft();
}

void OperatorStop()
{
    // MotorA
    GPIO_WriteBit(GPIOC, GPIO_Pin_6, 0);
    GPIO_WriteBit(GPIOC, GPIO_Pin_7, 0);

    // MotorB
    GPIO_WriteBit(GPIOC, GPIO_Pin_0, 0);
    GPIO_WriteBit(GPIOD, GPIO_Pin_3, 0);

    //MotorC
    GPIO_WriteBit(GPIOC, GPIO_Pin_2, 0);
    GPIO_WriteBit(GPIOC, GPIO_Pin_5, 0);

    //MotorD
    GPIO_WriteBit(GPIOD, GPIO_Pin_2, 0);
    GPIO_WriteBit(GPIOC, GPIO_Pin_1, 0);
}

